/*
 * TimedSolenoid.cpp
 *
 *  Created on: Feb 3, 2013
 *      Author: N500432
 */

#include "TimedSolenoid.h"

TimedSolenoid::TimedSolenoid(UINT32 port) :
	EntechSubsystem("TimedSolenoid")
{
	m_solenoid = new Solenoid(port);
	m_timer = new Timer();
	m_state = kNotTriggering;
}

TimedSolenoid::~TimedSolenoid()
{
}

void TimedSolenoid::Trigger(bool on)
{
	if (on) {
		m_state = kTriggeringOn;
	} else {
		m_state = kTriggeringOff;
	}
	m_timer->Stop();
	m_timer->Reset();
	m_timer->Start();
}

void TimedSolenoid::DoTriggering(void)
{
	switch (m_state) {
	case kNotTriggering:
		break;
	case kTriggeringOn:
		if (m_timer->Get() < c_timeout) {
			m_solenoid->Set(true);
		} else {
			m_state = kNotTriggering;
			m_timer->Stop();
		}
		break;
	case kTriggeringOff:
		if (m_timer->Get() < c_timeout) {
			m_solenoid->Set(false);
		} else {
			m_state = kNotTriggering;
			m_timer->Stop();
		}
		break;
	}
}

void TimedSolenoid::DoAutonomous(void)
{
	DoTriggering();
}

void TimedSolenoid::DoTeleop(void)
{
	DoTriggering();
}

void TimedSolenoid::DoTest(void)
{
	if (m_testEnabled) {
		DoTriggering();
	}
}
